#Rem
Header: A* path finder. 
#End
Strict

Import agfx

#Rem
summary:  TPathfinder class
#End
Class TPathfinder Abstract

	Global Used:Int = False
	
	#Rem
	summary: define type of path result	
	0=no diagonal movement.
	1=diagonal movement and cutting corners allowed.
	2=diagonal movement but no cutting corners.
	#End
	
	Global Diagonals:Int

	#Rem
	summary: The higher this number, the more the path will randomly differ from what is optimum.
	#End
	
	Global Randomity:Float
	
	#Rem
	summary: Map is a float. The closer to 1 the harder it is to move into this tile.
	All values 1 or greater are considered walls.
	#End
	
	Global Map:Float[][]
	
	#Rem
	summary: Map width
	#End
	Global MapWidth:Int
	
	#Rem
	summary: Map height
	#End
	Global MapHeight:Int
	
	#Rem
	summary: The amount of steps in the route. (Read only)
	#End
	
	Global Paths:Int
	
	#Rem
	summary: The resulting path is a 'resliced' route[]. 
	as [x0, y0, x1, y1, x2, y2, x3, y3, ..etc. .. xn,yn]
	The size of this array is paths*2.
	#End
	
	Global Route:Int[]
	
	#Rem
	summary: The higher the BasicCost, the more accurate and slow pathfinding will be.
	#End
	Const BasicCost:Float = .17
	
	
'Private:
	Global PathMap:TPath[][]
	Const Root2:Float = 1.4142
	
	#Rem
	summary: setup path
	 - must be called before FindPath
	 - Randomity must be positive.
	 - Diagonals must be 0 or 1 or 2.
	#End
	Function SetUp:Int(MapPassed:Float[][],mW%,mH%, Diag:Int=1, Random:Float=0)
		
		If Not(Diag = 0 Or Diag = 1 Or Diag = 2) Then Error("Astar: Diagonals must be 0 Or 1 Or 2.")
		
		
		Map = MapPassed
		MapWidth = mW
		MapHeight = mH
		Diagonals = Diag
		Randomity = Random
		Return 0
	End
	
	#Rem
	summary: Find path from A to B from vector2d
	Returns 1 if successful and 0 if unseccessful.
	Fills the route[] array if successful.
	#End
	
	Function FindPath:Void(a:TVector2,b:TVector2)
		FindPath(a.x,a.y,b.x,b.y)
	End

	#Rem
	summary: Find path from A to B from Ax,Ay,Bx,By
	Returns 1 if successful and 0 if unseccessful.
	Fills the route[] array if successful.
	#End
	Function FindPath:Int(StartX:Int, StartY:Int, EndX:Int, EndY:Int)
		
		If StartX < 0 Or StartY < 0 Or StartX >= MapWidth Or StartY >= MapHeight Then 
		 	Error("Astar: Starting point out of bounds: " + StartX + "," + StartY )
		Endif
			
		If EndX < 0 Or EndY < 0 Or EndX >= MapWidth Or EndY >= MapHeight Then 
		 	Error("Astar: End point out of bounds: " + EndX + "," + EndY )
		Endif	
		
		If Not Map Then
			Error("Astar: SetUp() must be called before FindPath" )
		Endif		
		
		'If already on target.
		If StartX = EndX And StartY = EndY Then
			Route = New Int[2]
			Route[0] = StartX
			Route[1] = StartY
			Paths = 1
			Return 1
		Endif
		
		Paths = 0
		
		'If target is a wall.
		If Map[EndX][EndY] >= 1 Then 
			Route = New Int[0]
			Return 0
		Endif
		
		Local P:TPath
		Local P2:TPath
		Local NewP:TPath
		Local NewX:Int
		Local NewY:Int
		Local Dir:Int
		Local DirMax:Int
		Local Done:Int
		Local PHead:TPath
		Local MapHere:Float
		Local DistX:Int
		Local DistY:Int
		
		PathMap = TPathArray2D(MapWidth,MapHeight)'[New TPath[MapWidth],New TPath[MapHeight]] '[MapWidth][MapHeight]
		
		'Make first path node at start.
		P = New TPath
		PHead = P
		P.X = StartX
		P.Y = StartY
		PathMap[StartX][StartY] = P
		
		If Diagonals Then
			DirMax = 7
		Else
			DirMax = 3
		Endif
		
		
		Repeat
			
			
			For Dir = 0 To DirMax
				
				'Move based on direction.
				Select Dir
					Case 0
						NewX = P.X + 1
						NewY = P.Y
					Case 1
						NewX = P.X    
						NewY = P.Y + 1
					Case 2
						NewX = P.X - 1
						NewY = P.Y
					Case 3
						NewX = P.X    
						NewY = P.Y - 1
					Case 4
						NewX = P.X + 1
						NewY = P.Y + 1
					Case 5
						NewX = P.X - 1
						NewY = P.Y + 1
					Case 6
						NewX = P.X - 1
						NewY = P.Y - 1
					Case 7
						NewX = P.X + 1
						NewY = P.Y - 1
				End'Select
				
				'Check if it is ok to make a new path node here.
				If NewX >= 0 And NewY >= 0 And NewX < MapWidth And NewY < MapHeight Then
					MapHere = Map[NewX][NewY]
					If MapHere < 1 Then
						
						'No cutting corners.
						If Diagonals = 2 And Dir > 3 Then
							If Map[NewX][P.Y] >= 1 Then Continue
							If Map[P.X][NewY] >= 1 Then Continue
						Endif
						
						P2 = PathMap[NewX][NewY]
						
						'Check if there already is a path here.
						If P2 = Null Then
							
							'DrawRect newx*29,newy*29,29,29
							'Flip False
							'If KeyHit(key_escape) Then End
							
							'Make new node.
							NewP = New TPath
							PathMap[NewX][NewY] = NewP
							NewP.Parent = P
							NewP.X = NewX
							NewP.Y = NewY
							
							'Cost is slightly more for diagnols.
							If Dir < 4 Then
								NewP.Cost = P.Cost + BasicCost + MapHere + Rnd(0, Randomity)
							Else
								NewP.Cost = P.Cost + (BasicCost + MapHere + Rnd(0, Randomity)) * Root2
							Endif
							
							'Calculate distance from this node to target.
							If Diagonals Then
								DistX = Abs(NewX - EndX)
								DistY = Abs(NewY - EndY)
								If DistX > DistY Then
									NewP.Dist = DistX - DistY + DistY * Root2
								Else
									NewP.Dist = DistY - DistX + DistX * Root2
								Endif
								NewP.Dist *= .1
							Else
								NewP.Dist = (Abs(NewX - EndX) + Abs(NewY - EndY)) / 8.0
							Endif
							
							'Insert node at appropriate spot in list.
							P2 = P
							Repeat
								If P2.After = Null Then
									P2.After = NewP
									Exit
								Endif
								If P2.After.Dist + P2.After.Cost > NewP.Dist + NewP.Cost Then
									NewP.After = P2.After
									P2.After = NewP
									Exit
								Endif
								P2 = P2.After
							Forever
							
							'Check if found the end.
							If NewX = EndX And NewY = EndY Then
								Done = 1
								Exit
							Endif
						Else
							'Overwrite existing path node if this way costs less.
							If P2.Cost > P.Cost + BasicCost + MapHere * Root2 + Randomity Then
								P2.Parent = P
								'Cost is slightly more for diagnols.
								If Dir < 4 Then
									P2.Cost = P.Cost + BasicCost + MapHere + Rnd(0, Randomity)
								Else
									P2.Cost = P.Cost + (BasicCost + MapHere + Rnd(0, Randomity)) * Root2
								Endif
							Endif
						Endif
					Endif
				Endif
			Next
			
			If Done = 1 Then Exit
			
			P = P.After
			If P = Null Then Exit
			
		Forever
		
		
		If Done Then
			'Count how many paths.
			P2 = NewP
			Repeat
				Paths+= 1
				P2 = P2.Parent
				If P2 = Null Then Exit
				'If KeyDown(key_space) Then DebugStop
			Forever
			
			'Make route from end to start.
			Route = New Int[Paths * 2]
			Local i:Int = 0
			P2 = NewP
			Repeat
				Route[i] = P2.X
				i+= 1
				Route[i] = P2.Y
				i+= 1
				P2 = P2.Parent
				If P2 = Null Then Exit
			Forever
		Endif
		
		'Nullify parent pointers so mem will be deallocated.
		P = PHead
		Repeat
			P.Parent = Null
			P = P.After
			If P = Null Then Exit
		Forever
		
		Return Done
	End
End


#Rem
	summary: Path class element
#End
Class TPath
	Field X:Int
	Field Y:Int
	Field Parent:TPath
	Field Cost:Float
	Field Dist:Float
	Field After:TPath
End


#Rem
	summary: Create 2D array of TPath
	[i]only for internal use[/i]
#End
Function TPathArray2D:TPath[][] (X%,Y%)
	Local tmp:TPath[X][]
	For Local I% = 0 Until X
		tmp[I] = New TPath[Y]
	Next
	Return tmp

End Function

#Rem
	Footer: Example
[code]
Strict

Import agfx

Global myAstarScreen:TAstarScreen

Class TAgfx_test_app Extends TFramework

	Method OnCreate:Int()
		
		'DEVICE.SetFps(30)		' default FPS is 60
		
		Super.OnCreate()
				
		'DISPLAY.SetVirtualSize(480,320)
		myAstarScreen = New TAstarScreen		
		myAstarScreen.Prepare()
		
		Return 0
	End
	
	
End


Class TAstarScreen Extends TScreen
	
	Field testMAP:Float[][]
	Field PStart:TVector2
	Field PEnd:TVector2


	' START when app is created
	Method Start:Void()



		Self.PStart = New TVector2(1,1)
		Self.PEnd = New TVector2(49,49)

		Self.testMAP = FloatArray2D(50,50)
		Self.MakeMap()
		
	End
	
	
	Method Update:Void()

		If KeyHit(KEY_1) Then
			TPathfinder.SetUp(testMAP,50,50,0)
			Print("0=no diagonal movement")							
		End 

		If KeyHit(KEY_2) Then
			TPathfinder.SetUp(testMAP,50,50,1)			
			Print("1=diagonal movement and cutting corners allowed")			
		End 

		If KeyHit(KEY_3) Then
			TPathfinder.SetUp(testMAP,50,50,2)
			Print("2=diagonal movement but no cutting corners")
		End 

		If MOUSE.Hit() Then
			Self.PStart.Set(MOUSE.x/10,MOUSE.y/10)
			Self.PEnd.Set(MOUSE.x/10,MOUSE.y/10)
			
		End

		If MOUSE.Down() Then
			
			Self.PEnd.Set(MOUSE.x/10 ,MOUSE.y/10)

		End

		If MOUSE.Up() Then
			
			Self.PEnd.Set(MOUSE.x/10 ,MOUSE.y/10)
			TPathfinder.FindPath(Self.PStart,Self.PEnd)
			
		End


		If KeyHit(KEY_SPACE) Then
			TPathfinder.FindPath(Self.PStart,Self.PEnd)			
		End

	End
	
	
	Method Render:Void()
		Cls(0,0,0)
		
		For Local I% = 0 Until 50
			For Local J% = 0 Until 50
				SetAlpha( Min(testMAP[I][J],1.0))
				DrawRect(Int(I*10),Int(J*10),9,9)
			Next
		Next	
		
		SetAlpha(1)
		SetColor(255,0,0)
		For Local Z% = 0 Until TPathfinder.Route.Length Step 2
			DrawRect(TPathfinder.Route[Z]*10, TPathfinder.Route[Z+1]*10,9,9)
		Next

		
		DrawLine(3+Self.PStart.x*10,3+Self.PStart.y*10,3+Self.PEnd.x*10,3+Self.PEnd.y*10)
		SetColor(255,0,0)
		DrawOval(3+Self.PStart.x*10,3+Self.PStart.y*10,4,4)
		SetColor(0,255,0)
		DrawOval(3+Self.PEnd.x*10,3+Self.PEnd.y*10,4,4)

		SetColor(255,255,255)
		
		DrawText("[1-3] - path mode",640-130,0)
		DrawText("[SPACE] - calc path",640-130,15)

	End Method


	Method MakeMap:Void()
		For Local I:Int = 0 Until 50
			For Local J:Int = 0 Until 50
				testMAP[I][J] = Rnd(0,1.5)
			Next
		Next	
		
		TPathfinder.SetUp(testMAP,50,50) 

	End Method


End



[/code]	
#End

